{
 "metadata": {
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": 3
  },
  "orig_nbformat": 2,
  "kernelspec": {
   "name": "python_defaultSpec_1595844130846",
   "display_name": "Python 3.7.6 64-bit ('base': conda)"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2,
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "output_type": "stream",
     "name": "stderr",
     "text": "Using TensorFlow backend.\nTensor(\"Placeholder:0\", shape=(?, ?, ?, 3), dtype=float32)\nTensor(\"conv5_3/conv5_3:0\", shape=(?, ?, ?, 512), dtype=float32)\nTensor(\"rpn_conv/3x3/rpn_conv/3x3:0\", shape=(?, ?, ?, 512), dtype=float32)\nTensor(\"lstm_o/Reshape_2:0\", shape=(?, ?, ?, 512), dtype=float32)\nTensor(\"lstm_o/Reshape_2:0\", shape=(?, ?, ?, 512), dtype=float32)\nTensor(\"rpn_cls_score/Reshape_1:0\", shape=(?, ?, ?, 20), dtype=float32)\nTensor(\"rpn_cls_prob:0\", shape=(?, ?, ?, ?), dtype=float32)\nTensor(\"Reshape_2:0\", shape=(?, ?, ?, 20), dtype=float32)\nTensor(\"rpn_bbox_pred/Reshape_1:0\", shape=(?, ?, ?, 40), dtype=float32)\nTensor(\"Placeholder_1:0\", shape=(?, 3), dtype=float32)\nTensor_name is :  rpn_conv/3x3/biases\nTensor_name is :  rpn_cls_score/weights\nTensor_name is :  rpn_bbox_pred/biases\nTensor_name is :  lstm_o/weights\nTensor_name is :  lstm_o/bidirectional_rnn/fw/lstm_cell/bias\nTensor_name is :  lstm_o/bidirectional_rnn/bw/lstm_cell/kernel\nTensor_name is :  lstm_o/bidirectional_rnn/bw/lstm_cell/bias\nTensor_name is :  conv5_3/weights\nTensor_name is :  conv5_3/biases\nTensor_name is :  lstm_o/biases\nTensor_name is :  conv5_2/weights\nTensor_name is :  conv2_2/weights\nTensor_name is :  conv1_1/weights\nTensor_name is :  conv4_2/weights\nTensor_name is :  conv2_2/biases\nTensor_name is :  conv2_1/weights\nTensor_name is :  rpn_cls_score/biases\nTensor_name is :  conv1_2/biases\nTensor_name is :  conv2_1/biases\nTensor_name is :  rpn_conv/3x3/weights\nTensor_name is :  conv3_2/biases\nTensor_name is :  conv3_1/weights\nTensor_name is :  conv4_3/weights\nTensor_name is :  conv1_2/weights\nTensor_name is :  conv4_1/biases\nTensor_name is :  rpn_bbox_pred/weights\nTensor_name is :  conv3_2/weights\nTensor_name is :  lstm_o/bidirectional_rnn/fw/lstm_cell/kernel\nTensor_name is :  conv3_3/biases\nTensor_name is :  conv3_3/weights\nTensor_name is :  conv4_1/weights\nTensor_name is :  conv1_1/biases\nTensor_name is :  conv4_2/biases\nTensor_name is :  conv3_1/biases\nTensor_name is :  conv4_3/biases\nTensor_name is :  conv5_1/biases\nTensor_name is :  conv5_1/weights\nTensor_name is :  conv5_2/biases\nload vggnet done\n"
    },
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": "Message()"
     },
     "metadata": {},
     "execution_count": 4
    }
   ],
   "source": [
    "from serial.tools import list_ports\n",
    "from test_final_maybe import camera_active\n",
    "from pydobot.dobot import Dobot\n",
    "import time\n",
    "\n",
    "port = list_ports.comports()[0].device\n",
    "device = Dobot(port=port, verbose=False)\n",
    "device._set_queued_cmd_clear()\n",
    "device.clear_all_alarms_state()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": "(216.65994262695312, -13.832086563110352, 146.98878479003906, -3.6529412269592285, -4.05294132232666, 4.6429243087768555, -4.8512959480285645, 0.0)\n"
    }
   ],
   "source": [
    "device.go_home()\n",
    "print(device.pose())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": "(-9.035144103108905e-06, -206.6999969482422, 135.0, -90.0, -90.0, 0.0, 0.0, 0.0)\n"
    }
   ],
   "source": [
    "time.sleep(4)\n",
    "device.rotate_to(-90.0,0.0,0.0,0.0,True)\n",
    "print(device.pose())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": "[[68.33333333333333, -64.84375], [-40.416666666666664, -20.3125], [-0.4166666666666667, 52.5]]\n"
    }
   ],
   "source": [
    "# device.go_home()\n",
    "\n",
    "positions = camera_active()\n",
    "print(positions)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "positions = [[68.33333333333333, -64.84375], [-40.416666666666664, -20.3125], [-0.4166666666666667, 52.5]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "positions = [[73.75, -63.59375000000001], [-25.624999999999996, -19.375], [-0.20833333333333334, 52.1875]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": "68.33333333333333 -162.15625\n-40.416666666666664 -206.6875\n-0.4166666666666667 -279.5\n"
    }
   ],
   "source": [
    "grab_position_adjust = -27\n",
    "base_y = -200\n",
    "for position in positions:\n",
    "    device.rotate_to(-90.0,0.0,0.0,0.0,True)\n",
    "    # print(device.pose()[:3])\n",
    "    # (x, y, z, r, j1, j2, j3, j4) = device.pose()\n",
    "    device.move_to(0 , -200,0, -90, wait=True)\n",
    "    grab_x,y = position\n",
    "    # grab_x = grab_x -(grab_x/100)*grab_x if grab_x >0 else grab_x +(grab_x/100)*grab_x\n",
    "    grab_y = -y + grab_position_adjust + base_y# limite <100\n",
    "    # print(grab_x,grab_y)\n",
    "    if grab_x>100:\n",
    "        grab_x = 100\n",
    "    elif grab_x<-100:\n",
    "        grab_x = -100\n",
    "    if grab_y>-130:\n",
    "        grab_y = -130\n",
    "    elif grab_y<-280:\n",
    "        grab_y = -280\n",
    "    if grab_x <0 and grab_y <0 and (grab_x/grab_y<1.2 and grab_x/grab_y>0.8):\n",
    "        grab_y = -200\n",
    "        grab_x = 0\n",
    "    \n",
    "    print(grab_x,grab_y)\n",
    "    grab_z = -31\n",
    "    z = 0\n",
    "        \n",
    "    device.move_to(-grab_x,grab_y,z,-90,wait=True)\n",
    "    time.sleep(1)\n",
    "    # print(device.pose()[:3])\n",
    "    # device.move_to(grab_x,grab_y,grab_z,r,wait=False)\n",
    "    # device.suck(True)\n",
    "    # device.rotate_to(0.0,0.0,0.0,0.0)\n",
    "    # device.suck(False)\n",
    "    device.rotate_to(-90.0,0.0,0.0,0.0,True)\n",
    "    # print(device.pose()[:3])\n",
    "    # device.move_to(300 , -83, grab_z, r, wait=True)\n",
    "    device._set_queued_cmd_clear()\n",
    "    device.clear_all_alarms_state()\n",
    "    # break"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "x = -100\n",
    "y = -130\n",
    "\n",
    "device.move_to(0 ,-200,0, -90, wait=True)\n",
    "device.move_to(x,y,0,-90,wait=True)\n",
    "device.rotate_to(-90.0,0.0,0.0,0.0,True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "device.rotate_to(-90.0,0.0,0.0,0.0,True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ]
}